/********************************************************************************
 * Copyright(c) 2020-2025 BST All rights reserved.
 * - Filename
 * - Author
 * - Version V1.0.0
 * - Date    2024/08/28
 * - Brief
 * - FunctionList:
 ******************************************************************************
 * History:
 *
 *
 *
 ******************************************************************************
 */
#include "LsLidar128s2Adapter.h"
#include <sys/time.h>
#include "TimeUtil.h"

using namespace lidar;
using namespace lidar::lslidar;

LsLidar128s2Adapter::LsLidar128s2Adapter(const uint32_t &id, const uint16_t &msop_port,
                                         const uint16_t &difop_port)
    : _id(id), _msopPort(msop_port), _difopPort(difop_port)
{
  std::string info;
  info.append("Id:");
  info.append(std::to_string(_id));
  info.append(", IP:");
  info.append(_ip);
  info.append(", MsopPort:");
  info.append(std::to_string(_msopPort));
  info.append(", DifoPort:");
  info.append(std::to_string(_difopPort));

  std::cout << "LsLidar128s2Adapter  create : " << info << std::endl;
}

LsLidar128s2Adapter::~LsLidar128s2Adapter()
{
  std::cout << "LsLidar128s2Adapter Destroy..." << std::endl;
}

void LsLidar128s2Adapter::start(
    std::function<void(std::shared_ptr<lpc::LidarProtoPointCloud> &&)> callback)
{
  _isExit = false;
  _lidar = std::make_shared<GetLidarData_LS>();
  _lidar->setPortAndIP(_msopPort, _difopPort, "127.0.0.1", _ip, "226.1.1.102");

  _lidar->start();

  _lidarFrameThread.reset(
      new std::thread(std::bind(&LsLidar128s2Adapter::procssLoop, this, callback)));
  _lidarGetFrame.reset(
      new std::thread(std::bind(&LsLidar128s2Adapter::getLidarFrameLoop, this)));
}

void LsLidar128s2Adapter::stop()
{
  _lidar->stop();
  _isExit = true;
  _condition.notify_all();
  if (_lidarFrameThread != nullptr && _lidarFrameThread->joinable())
  {
    _lidarFrameThread->join();
  }

  if (_lidarGetFrame != nullptr && _lidarGetFrame->joinable())
  {
    _lidarGetFrame->join();
  }

  if (_packageThread != nullptr && _packageThread->joinable())
  {
    _packageThread->join();
  }
  std::cout << "LsLidar128s2Adapter Exit ..." << std::endl;
}

void LsLidar128s2Adapter::processLoop(
    const std::function<void(std::shared_ptr<lpc::LidarProtoPointCloud> &&)>
        &callback)
{

  while (!_isExit)
  {
    std::unique_lock<std::mutex> lck(_mtx);
    _condition.wait(lck,
                    [this]
                    { return _isExit || !_lidarFrameDeque.empty(); });
    if (_isExit)
      break;
    auto lidarFrame = _lidarFrameDeque.front();
    _lidarFrameDeque.pop_front();
    lck.unlock();
    if (_isExit)
      break;

    // if (TimeUtil::fps_calc_inc(_fps)) {
    //   std::cout << "[FPS]: lidar" << lidarFrame->sensor_id
    //             << ", point count: " << lidarFrame->point_count
    //             << " total fps:" << _fps.avg_fps
    //             << ", fps: " << _fps.ins_fps << std::endl;
    // }
    callback(std::move(lidarFrame));
  }
}

void LsLidar128s2Adapter::getLidarFrameLoop()
{
  while (!_isExit)
  {
    if (_lidar->isFrameOK)
    {
      std::shared_ptr<std::vector<MuchLidarData>> m_LidarData;
      std::string mInfo;
      if (!_lidar->getLidarPerFrameData(m_LidarData, mInfo))
      {
        std::this_thread::sleep_for(std::chrono::milliseconds(1));
        continue;
      }

      auto frame = std::make_shared<lpc::LidarProtoPointCloud>();
      frame->set_lidar_id(_id);
      frame->set_system_timestamp(TimeUtil::now_us());
      frame->set_timestamp(tm / 1000); // ns to us
      frame->set_seq(0);
      frame->set_height(m_LidarData->size());
      frame->set_width(1);
      frame->set_is_dense(false);
      uint64_t timestamp = frame->system_timestamp();
      for (int i = 0; i < m_LidarData->size(); i++)
      {
        frame->add_data(m_LidarData->at(i).X);
        frame->add_data(m_LidarData->at(i).Y);
        frame->add_data(m_LidarData->at(i).Z);
        frame->add_data(m_LidarData->at(i).Intensity);
        if (i == 0)
        {
          /* ns to us*/
          timestamp = m_LidarData->at(i).Mtimestamp_nsce / 1000;
        }
      }
      frame->set_timestamp(timestamp);
    }
    else
    {
      std::this_thread::sleep_for(std::chrono::milliseconds(1));
    }

    {
      std::unique_lock<std::mutex> lck(_mtx);
      _lidarFrameDeque.emplace_back(frame);
    }
    _condition.notify_one();
  }
}

void LsLidar128s2Adapter::lidarFrameCallback(const std::shared_ptr<std::vector<MuchLidarData>> data, int count, std::string info)
{
  auto frame = std::make_shared<lpc::LidarProtoPointCloud>();
  frame->set_lidar_id(_id);
  frame->set_system_timestamp(TimeUtil::now_us());
  frame->set_timestamp(tm / 1000); // ns to us
  frame->set_seq(0);
  frame->set_height(data->size());
  frame->set_width(1);
  frame->set_is_dense(false);
  uint64_t timestamp = frame->system_timestamp();
  for (int i = 0; i < data->size(); i++)
  {
    frame->add_data(data->at(i).X);
    frame->add_data(data->at(i).Y);
    frame->add_data(data->at(i).Z);
    frame->add_data(data->at(i).Intensity);
    if (i == 0)
    {
      /* ns to us*/
      timestamp = data->at(i).Mtimestamp_nsce / 1000;
    }
  }
  frame->set_timestamp(timestamp);
  {
    std::unique_lock<std::mutex> lck(_mtx);
    _lidarFrameDeque.emplace_back(frame);
  }
  _condition.notify_one();
}
